#include "Navmesh.h"

Navmesh::~Navmesh()
{
    clear();
}

void Navmesh::clear()
{
    for (auto it = trilist.begin(); it != trilist.end();)
    {
        delete *it;
        it = trilist.erase(it);
    }
    for (auto it = edgelist.begin(); it != edgelist.end();)
    {
        delete *it;
        it = edgelist.erase(it);
    }
    edgelist.clear();
    plist.clear();
}

void Navmesh::generate()
{
    assert(plist.size() >= 3);
    assert(boundVertexs.size() >= 3);
    addRestrainPolygon(boundVertexs);
    _triangulate(plist);
}

real Navmesh::getTriangleMinAngle(const Triangle *tri)
{
    const Vector2 *ps = tri->getPoints();
    real min = (ps[1] - ps[0]).length2();
    int index = 0;
    for (int i = 1; i < 3; ++i)
    {
        real temp = (ps[(i + 1) % 3] - ps[i]).length2();
        if (temp < min)
        {
            min = temp;
            index = i;
        }
    }
    // Vector2 line[3][2];
    int index2 = (index + 1) % 3;
    int index3 = (index + 2) % 3;
    // Log::log("e:%d %d\n", index2, index3);
    Vector2 dt1 = (ps[index2] - ps[index]);
    Vector2 dt2 = (ps[index2] - ps[index3]);
    Vector2 dt = dt2 - dt1;
    real rad = so_atan2(dt.y, dt.x);
    // Log::log("rad:%f dt:%f,%f\n", rad, dt.x, dt.y);
    return so_abs(rad) * 180 / PI;
}

const std::vector<NavTriangle *> *Navmesh::getTriangles() const
{
    return &trilist;
}

int Navmesh::getContainTrigleIndex(Vector2 p)
{
    for (int i = 0; i < trilist.size(); ++i)
    {
        const Vector2 *ps = trilist[i]->getPoints();
        Vector2 ap = p - ps[0];
        Vector2 ab = ps[1] - ps[0];
        Vector2 ac = ps[2] - ps[0];
        real u = (ap * ab) / (ac * ab);
        real v = (ap * ac) / (ab * ac);
        real uv = u + v;
        if (u > 0 && v > 0 && uv < 1)
        {
            return i;
        }
    }
    return -1;
}

NavTriangle *Navmesh::getContainTrigle(Vector2 p) const
{
    for (int i = 0; i < trilist.size(); ++i)
    {
        const Vector2 *ps = trilist[i]->getPoints();
        Vector2 ap = p - ps[0];
        Vector2 ab = ps[1] - ps[0];
        Vector2 ac = ps[2] - ps[0];
        real u = (ap * ab) / (ac * ab);
        real v = (ap * ac) / (ab * ac);
        real uv = u + v;
        if (u >= 0 && v >= 0 && uv < 1)
        {
            return trilist[i];
        }
    }
    return nullptr;
}


void Navmesh::addRestrainEdge(Edge e)
{
    addPoint(e.p1);
    addPoint(e.p2);
    elist.push_back(e);
}

void Navmesh::addRestrainPolygon(const std::vector<Vector2> ps)
{
    assert(ps.size() >= 3);
    for (int i = 0; i < ps.size(); ++i)
    {
        addRestrainEdge(Edge(ps[i], ps[(i + 1) % ps.size()]));
    }
}

const std::vector<Edge *> *Navmesh::getEdge() const
{
    return &edgelist;
}

void Navmesh::addPoint(Vector2 p)
{
    bool b = true;
    for (auto it = plist.begin(); it != plist.end(); ++it)
    {
        if ((*it) == p)
        {
            b = false;
        }
    }
    if (b)
    {
        plist.push_back(p);
    }
    if (plist.size() < 3)
    {
        return;
    }
}

void Navmesh::_triangulate(std::vector<Vector2> ps)
{
    if (ps.size() < 3)
    {
        return;
    }
    for (auto it = trilist.begin(); it != trilist.end();)
    {
        delete *it;
        it = trilist.erase(it);
    }
    for (auto it = edgelist.begin(); it != edgelist.end();)
    {
        delete *it;
        it = edgelist.erase(it);
    }
    real minX = ps[0].x;
    real minY = ps[0].y;
    real maxX = minX;
    real maxY = minY;

    for (std::size_t i = 0; i < ps.size(); ++i)
    {
        if (ps[i].x < minX)
            minX = ps[i].x;
        if (ps[i].y < minY)
            minY = ps[i].y;
        if (ps[i].x > maxX)
            maxX = ps[i].x;
        if (ps[i].y > maxY)
            maxY = ps[i].y;
    }

    const real dx = maxX - minX;
    const real dy = maxY - minY;
    const real deltaMax = so_max(dx, dy);
    const real midx = (minX + maxX) / 2;
    const real midy = (minY + maxY) / 2;

    const Vector2 p1(midx - 20 * deltaMax, midy - deltaMax);
    const Vector2 p2(midx, midy + 20 * deltaMax);
    const Vector2 p3(midx + 20 * deltaMax, midy - deltaMax);

    trilist.push_back(new NavTriangle(p1, p2, p3));
    for (auto p = begin(ps); p != end(ps); p++)
    {
        std::vector<Edge> polygon;

        for (auto it = trilist.begin(); it != trilist.end();)
        {
            auto tri = *it;
            if ((tri->getCircleCenter() - *p).length2() < tri->getRadius2())
            {
                const Vector2 *trips = tri->getPoints();
                for (int i = 0; i < 3; ++i)
                {
                    polygon.push_back(Edge{trips[i], trips[(i + 1) % 3]});
                }
                it = trilist.erase(it);
            }
            else
            {
                ++it;
            }
        }
        for (auto e1 = begin(polygon); e1 != end(polygon); ++e1)
        {
            for (auto e2 = e1 + 1; e2 != end(polygon); ++e2)
            {
                if (e1->equal(*e2))
                {
                    e1->isBad = true;
                    e2->isBad = true;
                }
            }
        }
        for (auto it = polygon.begin(); it != polygon.end();)
        {
            if (it->isBad)
            {
                it = polygon.erase(it);
            }
            else
            {
                ++it;
            }
        }

        for (auto e : polygon)
        {
            trilist.push_back(new NavTriangle((e).p1, (e).p2, *p));
            // edgelist.push_back(new Edge{e.p1, *p});
            // edgelist.push_back(new Edge{e.p2, *p});
            // edgelist.push_back(new Edge{e.p1, e.p2});
            // Log::log("size:%d\n",edgelist.size());
        }
        // Log::log("size:%d--\n",edgelist.size());
    }

    // 生成边 三角形邻边关系;
    for (auto it = trilist.begin(); it != trilist.end(); ++it)
    {
        auto tri = *it;
        const Vector2 *ps = tri->getPoints();
        for (int i = 0; i < 3; ++i)
        {
            Edge *e = new Edge(ps[i], ps[(i + 1) % 3]);
            bool b = true;
            for (auto it2 = edgelist.begin(); it2 != edgelist.end(); ++it2)
            {
                if ((*it2)->equal(*e))
                {
                    b = false;
                    break;
                }
            }
            if (b)
            {
                edgelist.push_back(e);
            }
            else
            {
                delete e;
            }
        }
    }

    for (auto it = trilist.begin(); it != trilist.end(); ++it)
    {
        for (auto it2 = edgelist.begin(); it2 != edgelist.end(); ++it2)
        {
            for (auto it3 = elist.begin(); it3 != elist.end(); ++it3)
            {
                if (it3->equal(**it2))
                {
                    (*it2)->isRestrain = true;
                }
            }
            const Vector2 *ps = (*it)->getPoints();
            for (int i = 0; i < 3; i++)
            {
                if (Edge(ps[i], ps[(i + 1) % 3]).equal(**it2))
                {
                    (*it2)->tris.push_back(*it);
                    (*it)->edges.push_back(*it2);
                };
            }
        }
    }
    // int d1 = 0;
    // int d2 = 0;
    // for (int i = 0; i < edgelist.size(); ++i)
    // {
    //     d1 += edgelist[i]->tris.size();
    // }
    // for (int i = 0; i < trilist.size(); ++i)
    // {
    //     d2 += trilist[i]->edges.size();
    // }
    // Log::log("%d %d\n", d1, d2);
}

void Navmesh::setBoundVertexs(const std::vector<Vector2> ps)
{
    boundVertexs = ps;
}

std::vector<NavPathTriangle *> Navmesh::findPathTriangles(Vector2 start, Vector2 end, real diameter)
{
    std::vector<NavPathTriangle *> path;
    if (start == end || getContainTrigle(start) == getContainTrigle(end))
    {
        return path;
    }
    std::priority_queue<NavPathTriangle *, std::vector<NavPathTriangle *>, NavPathTriangleCompare> que;
    NavTriangle *tri = getContainTrigle(start);
    if (tri == nullptr)
    {
        return path;
    }
    NavPathTriangle *npt = tri->createNavPathTriangle();
    que.push(npt);
    std::unordered_map<Edge *, int> hash;
    npt->h = 0;
    npt->cost = npt->g + npt->h;
    while (!que.empty())
    {
        NavPathTriangle *cur = que.top();
        que.pop();
        if (getContainTrigle(end) == cur->parent)
        {
            // 找到了
            while (cur != nullptr)
            {
                path.push_back(cur);
                cur = cur->from;
            }

            for (int i = 0; i < path.size() / 2; i++)
            {
                NavPathTriangle *temp = path[i];
                path[i] = path[path.size() - i - 1];
                path[path.size() - i - 1] = temp;
            }

            if (path.size() >= 2)
            {
                for (int i = 0; i < path.size() - 1; ++i)
                {
                    path[i]->nextEdge = getCommonEdge(path[i]->parent, path[i + 1]->parent)->createNavPathEdge();
                }
            }
            return path;
        }
        auto edges = cur->parent->edges;
        for (auto it = edges.begin(); it != edges.end(); ++it)
        {
            auto e = *it;
            if (!e->isRestrain && hash.find(e) == hash.end() && (e->p1 - e->p2).length2() >= diameter * diameter)
            {
                auto tris = e->tris;
                for (int i = 0; i < tris.size(); ++i)
                {
                    if (tris[i] != cur->parent)
                    {
                        NavPathTriangle *npt = tris[i]->createNavPathTriangle();
                        que.push(npt);
                        npt->from = cur;
                        npt->g = cur->g + (cur->parent->getCenter() - npt->parent->getCenter()).length();
                        npt->h = (end - npt->parent->getCenter()).length();
                        npt->cost = npt->g + npt->h;
                        npt->nextEdge = e->createNavPathEdge();
                        hash.emplace(e, 1);
                    }
                }
            }
        }
    }

    return path;
}

std::stack<Vector2> Navmesh::findPath(CollisionEntity *agent, Vector2 target)
{
    real radius = so_max(agent->getAABB()->getWidth(), agent->getAABB()->getHeight()) / 2;
    CollisionEntityCircle *cir = dynamic_cast<CollisionEntityCircle *>(agent);
    if (cir)
    {
        radius = cir->getRadius();
    }
    std::vector<Vector2> path;
    std::stack<Vector2> res;
    std::unordered_set<const Edge *> edges = widthpathDetection(agent->getPosition(), target, radius*2);
    if (edges.empty())
    {
        return res;
    }
    // Log::log("ray:%d\n", edges.size());
    auto list = findPathTriangles(agent->getPosition(), target, radius * 2);
    if (list.empty())
    {
        return res;
    }
    for (int i = 0; i < list.size(); ++i)
    {
        if (list[i]->nextEdge)
        {
            NavPathEdge *e = list[i]->nextEdge;
            // 是否逆时针
            if ((e->p1 - list[i]->parent->getCenter()) * (e->p2 - e->p1) < 0)
            {
                // 从右穿入
                Vector2 t = e->p1;
                e->p1 = e->p2;
                e->p2 = t;
            }
        }
    }
    path.push_back(agent->getPosition());
    int index = 0;
    std::vector<int> cross;
    bool left;
    while (path.back() != target)
    {
        Vector2 pt;
        // 每次找到一个拐点
        cross.clear();
        Vector2 p = _findTurnPoint(path.back(), target, list, index, &cross, &left);
        if (!cross.empty() && p != target)
        {
            if (cross.size() == 1)
            {
                pt = list[cross[0]]->nextEdge->p2 - list[cross[0]]->nextEdge->p1;
                if (!left)
                {
                    pt = -pt;
                }
            }
            else
            {
                for (int i = 0; i < cross.size(); ++i)
                {
                    pt += list[cross[i]]->parent->getCircleCenter();
                }
                pt = (pt / cross.size() - p);
            }
            pt = pt.normalized() * radius;
            // Log::log("pt:%d ", cross.size());
        }

        path.push_back(p + pt);
    }
    // Log::log("size %d ", path.size());
    for (auto it = list.begin(); it != list.end();)
    {
        NavPathTriangle *npt = *it;
        delete npt->nextEdge;
        delete npt;
        it = list.erase(it);
    }
    for(int i=0;i<path.size();++i){
        res.push(path[path.size()-i-1]);
    }
    return res;
}

Edge *Navmesh::getCommonEdge(NavTriangle *t1, NavTriangle *t2)
{
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
        {
            if (t1->edges[i]->equal(*t2->edges[j]))
            {
                return t1->edges[i];
            }
        }

    return nullptr;
}

std::unordered_set<const Edge *> Navmesh::widthpathDetection(Vector2 from, Vector2 to, real width, int max)
{
    real dt = width / max;
    Vector2 f, t;
    Vector2 dv = to - from;
    Vector2 v = Vector2(dv.y, -dv.x).normalized();
    Vector2 dd = v * width / 2;
    std::unordered_set<const Edge*> set;
    for (int i = 0; i < max; ++i)
    {
        Vector2 m = (dd + v * dt * i);
        f = from + m;
        t = to + m;
        auto set2 = pathDetection(f, t);
        for(auto it=set2.begin();it!=set2.end();++it){
            set.emplace(*it);
        }
    }
    return set;
}

std::unordered_set<const Edge *> Navmesh::pathDetection(Vector2 from, Vector2 to)
{
    std::unordered_set<const Edge *> set;
    for (auto it = edgelist.begin(); it != edgelist.end(); ++it)
    {
        if ((*it)->isRestrain == false)
        {
            continue;
        }
        Vector2 a = (*it)->p1;
        Vector2 b = (*it)->p2;

        if (((from - a) * (from - to)) * ((from - b) * (from - to)) < 0 && ((from - a) * (a - b)) * ((to - a) * (a - b)) < 0)
        {
            set.emplace(*it);
        }
    }
    return set;
}

Vector2 Navmesh::_findTurnPoint(Vector2 ori, Vector2 target, const std::vector<NavPathTriangle *> &list, int &index, std::vector<int> *crossTriangles, bool *turnLeft)
{
    if (index >= list.size() - 1)
    {
        return target;
    }
    Vector2 ab; // 左边缘
    Vector2 ac; // 右边缘
    Vector2 l;  // 左行进点
    Vector2 r;  // 右行进点
    Vector2 pl; // 左点
    Vector2 pr; // 右点
    Vector2 t;  // 临时探测向量
    std::unordered_map<int, Vector2> map;
    int backl = index;
    int backr = index;
    bool left = true;
    for (int i = index; i < list.size();)
    {
        NavPathEdge *e = nullptr;
        // 遍历到最后一个三角形时,把左右探测点都设为目标点
        if (left)
        {
            e = list[i]->nextEdge;
            if (i == list.size() - 1)
            {
                pl = target;
                pr = target;
            }
            else
            {
                pl = e->p1;
                pr = e->p2;
            }

            // 更新初始数据
            if (i == index)
            {
                l = pl;
                r = pr;
                ab = pl - ori;
                ac = pr - ori;
                i++;
                continue;
            }
            t = pl - ori;
            if (t * ac <= 0)
            {
                while (backr <= i)
                {
                    if (r == list[backr]->nextEdge->p2)
                    {
                        if (crossTriangles)
                        {
                            crossTriangles->push_back(backr);
                        }
                        backr++;
                    }
                    else
                    {
                        break;
                    }
                }
                index = backr;
                if (turnLeft)
                {
                    *turnLeft = false;
                }
                return r;
            }
            else if (pl != l && (ab * t) * (ac * t) <= 0)
            {
                ab = t;
                l = pl;
                backl = i;
            }
        }
        else
        {
            t = pr - ori;
            if (t * ab >= 0)
            {
                while (backl <= i)
                {
                    if (l == list[backl]->nextEdge->p1)
                    {
                        if (crossTriangles)
                        {
                            crossTriangles->push_back(backl);
                        }
                        backl++;
                    }
                    else
                    {
                        break;
                    }
                }
                index = backl;
                if (turnLeft)
                {
                    *turnLeft = true;
                }
                return l;
            }
            else if (pr != r && (ab * t) * (ac * t) <= 0)
            {
                ac = t;
                r = pr;
                backr = i;
            }
            ++i;
        }
        left = !left;
    }
    return target;
}

NavTriangle::NavTriangle(Vector2 p1, Vector2 p2, Vector2 p3) : Triangle(p1, p2, p3)
{
}

NavPathTriangle *NavTriangle::createNavPathTriangle()
{
    NavPathTriangle *p = new NavPathTriangle();
    p->parent = this;
    p->g = 0;
    p->h = 0;
    p->cost = 0;
    p->from = nullptr;
    return p;
}

Edge::Edge(Vector2 p1, Vector2 p2)
{
    if (p1.y > p2.y)
    {
        this->p1 = p2;
        this->p2 = p1;
    }
    else if (p1.y < p2.y)
    {
        this->p1 = p1;
        this->p2 = p2;
    }
    else if (p1.x > p2.x)
    {
        this->p1 = p2;
        this->p2 = p1;
    }
    else
    {
        this->p1 = p1;
        this->p2 = p2;
    }
}

bool Edge::equal(const Edge &e) const
{
    return p1 == e.p1 && p2 == e.p2 || p2 == e.p1 && p1 == e.p2;
}

NavPathEdge *Edge::createNavPathEdge()
{
    return new NavPathEdge{p1, p2};
}